from abc import ABC
import numpy as np


class TransformationUtils(ABC):

    @staticmethod
    def Txyz(tx, ty, tz):
        T = np.eye(4)
        T[:3, 3] = [tx, ty, tz]
        return T

    @staticmethod
    def Rx(phi):
        return np.array([
            [1, 0, 0, 0],
            [0, np.cos(phi), -np.sin(phi), 0],
            [0, np.sin(phi), np.cos(phi), 0],
            [0, 0, 0, 1]
        ])

    @staticmethod
    def Ry(theta):
        return np.array([
            [np.cos(theta), 0, np.sin(theta), 0],
            [0, 1, 0, 0],
            [np.sin(theta), 0, np.cos(theta), 0],
            [0, 0, 0, 1]
        ])

    @staticmethod
    def Rz(psi):
        return np.array([
            [np.cos(psi), -np.sin(psi), 0, 0],
            [np.sin(psi), np.cos(psi), 0, 0],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ])

    @staticmethod
    def RPY(phi, theta, psi):
        return TransformationUtils.Rz(psi) @ TransformationUtils.Ry(theta) @ TransformationUtils.Rx(phi)
